//
// Created by jiang on 2022/1/4.
//
#include "filter.h"
#include "string.h"
#include "stdlib.h"
void MID_Init(mid_t *pHandle, uint8_t num){
    pHandle->num = num;
    pHandle->src = (int *)malloc(sizeof(int) * num);
    pHandle->dest = (int *)malloc(sizeof(int) * num);
    pHandle->index = 0;
    pHandle->full = 0;
}

int MID_Filter(mid_t *pHandle, int x0) {
    pHandle->src[pHandle->index++] = x0;
    memcpy(pHandle->dest, pHandle->src, sizeof(int) * pHandle->num);

    if(pHandle->index == pHandle->num){
        pHandle->index = 0;
        if(!pHandle->full)
            pHandle->full = 1;
    }

    if(pHandle->full){
        memcpy(pHandle->dest, pHandle->src, sizeof(int) * pHandle->num);
        for (uint8_t i = 0; i < pHandle->num - 1; i++)
        {
            for (uint8_t j = 0; j < pHandle->num - i - 1; j++)
            {
                if (pHandle->dest[j] > pHandle->dest[j + 1])
                {
                    int dataTemp = pHandle->dest[j];
                    pHandle->dest[j] = pHandle->dest[j + 1];
                    pHandle->dest[j + 1] = dataTemp;
                }
            }
        }
        if(pHandle->num % 2)
            return pHandle->dest[pHandle->num / 2];
        else
            return (pHandle->dest[pHandle->num / 2] + pHandle->dest[pHandle->num / 2 - 1]) / 2;
    }else{
        memcpy(pHandle->dest, pHandle->src, sizeof(int) * pHandle->index);
        for (uint8_t i = 0; i < pHandle->index - 1; i++)
        {
            for (uint8_t j = 0; j < pHandle->index - i - 1; j++)
            {
                if (pHandle->dest[j] > pHandle->dest[j + 1])
                {
                    int dataTemp = pHandle->dest[j];
                    pHandle->dest[j] = pHandle->dest[j + 1];
                    pHandle->dest[j + 1] = dataTemp;
                }
            }
        }
        if(pHandle->index % 2)
            return pHandle->dest[pHandle->index / 2];
        else
            return (pHandle->dest[pHandle->index / 2] + pHandle->dest[pHandle->index / 2 - 1]) / 2;
    }
}

void MEAN_Init(mean_t *pHandle, uint8_t num) {
    pHandle->num = num;
    pHandle->array = (int *)malloc(sizeof(int) * num);
    pHandle->index = 0;
    pHandle->full = 0;
    pHandle->sum = 0;
}

int MEAN_Filter(mean_t *pHandle, int x0) {
    pHandle->sum += x0;
    pHandle->old = pHandle->array[pHandle->index];
    pHandle->array[pHandle->index++] = x0;

    if(pHandle->index == pHandle->num){
        pHandle->index = 0;
        if(!pHandle->full)
            pHandle->full = 1;
    }
    if(pHandle->full){
        pHandle->sum -= pHandle->old;
        return pHandle->sum / pHandle->num;
    }else{
        return pHandle->sum / pHandle->index;
    }
}


void IIRF_Init(iirf_t *pHandle, const float *num, const float *den) {
    pHandle->result = 0;
    for (uint8_t i = 0; i < IIR_NL; i++) {
        pHandle->nume[i] = *num;
        pHandle->deno[i] = *den;
        num++;
        den++;
    }
    memset(pHandle->inBuff, 0, sizeof(pHandle->inBuff) / sizeof(float));
    memset(pHandle->outBuf, 0, sizeof(pHandle->outBuf) / sizeof(float));
}


int IIRF_Filter(iirf_t *pHandle, float x0) {
    uint8_t i;
    pHandle->result = 0;
    pHandle->inBuff[0] = x0;

    for (i = 0; i < IIR_NL; i++)
        pHandle->result += pHandle->nume[i] * pHandle->inBuff[i];

    for (i = 1; i < IIR_NL; i++)
        pHandle->result -= pHandle->deno[i] * pHandle->outBuf[i];

    pHandle->outBuf[0] = pHandle->result;

    for (i = IIR_NL - 1; i > 0; i--) {
        pHandle->inBuff[i] = pHandle->inBuff[i - 1];
        pHandle->outBuf[i] = pHandle->outBuf[i - 1];
    }
    return (int) pHandle->outBuf[0];
}

void IIR_Init(iir_t *pHandle, const int16_t *num, const int16_t *den) {
    pHandle->result = 0;
    for (uint8_t i = 0; i < IIR_NL; i++) {
        pHandle->nume[i] = *num;
        pHandle->deno[i] = *den;
        num++;
        den++;
    }
    memset(pHandle->inBuff, 0, sizeof(pHandle->inBuff) / sizeof(int));
    memset(pHandle->outBuf, 0, sizeof(pHandle->outBuf) / sizeof(int));
}


int IIR_Filter(iir_t *pHandle, int x0) {
    uint8_t i;
    pHandle->result = 0;
    pHandle->inBuff[0] = x0;

    for (i = 0; i < IIR_NL; i++)
        pHandle->result += pHandle->nume[i] * pHandle->inBuff[i];

    for (i = 1; i < IIR_NL; i++)
        pHandle->result -= pHandle->deno[i] * pHandle->outBuf[i];

    pHandle->outBuf[0] = pHandle->result / (1 + pHandle->deno[0]);

    for (i = IIR_NL - 1; i > 0; i--) {
        pHandle->inBuff[i] = pHandle->inBuff[i - 1];
        pHandle->outBuf[i] = pHandle->outBuf[i - 1];
    }
    return pHandle->outBuf[0];
}


/***********************************************

预测部分
X(k|k-1) = A X(k-1|k-1)+B U(k)                  (1): X(k|k-1)是利用上一状态预测的结果、X(k-1|k-1)是上一状态最优的结果、A和B是预测系统参数(矩阵)、U(k)为现在状态的控制量
P(k|k-1) = A P(k-1|k-1) A’+ Q                   (2): P(k|k-1)是X(k|k-1)对应的协方差(先验)、P(k-1|k-1)是X(k-1|k-1)对应的协方差、Q是系统预测过程的协方差

最优解部分
X(k|k)= X(k|k-1) + Kg(k)(Z(k)-H X(k|k-1))        (3): X(k|k)是当前最优的结果、Kg(k)是卡尔曼增益、Z(k)是当前的测量值
Kg(k) = P(k|k-1) H’ / (H P(k|k-1) H’ + R)        (4): Kg(k)是卡尔曼增益、P(k|k-1)是X(k|k-1)对应的协方差、H是测量系统的参数(矩阵)、R是测量的噪声
P(k|k)=（I-Kg(k) H）P(k|k-1)                      (5): P(k|k)是X(k|k)对应的协方差:当前最优解的协方差(后验)

************************************************

适用于一维
X(k|k-1) = X(k-1|k-1)                               (1)
P(k|k-1) = P(k-1|k-1) + Q							(2)

X(k|k) = X(k|k-1) + Kg(k) (Z(k) - X(k|k-1))			(3)
Kg(k)  = P(k|k-1) / (P(k|k-1) + R)					(4)
P(k|k) =（1-Kg(k)）P(k|k-1)							(5)

************************************************/

/**
* @brief 初始化滤波器的初始值
* @param pHandle    : 滤波器指针
* @param Q          : 预测噪声方差 由系统外部测定给定
* @param R          : 测量噪声方差 由系统外部测定给定
*/
void Kalman_Init(kalman_t *pHandle, int Q, int R) {
    pHandle->A = 1;            //预测系统参数
    pHandle->H = 1;            //测量系统的参数
    pHandle->P = 10;           //后验状态估计值误差的方差的初始值（不要为0问题不大）
    pHandle->Q = Q;            //预测（过程）噪声方差 影响收敛速率，可以根据实际需求给出
    pHandle->R = R;            //测量（观测）噪声方差 可以通过实验手段获得
}

/**
* @brief 滤波迭代过程
* @param pHandle    : 滤波器指针
* @param x0         : 当前测定值
* @param y1         : 上次最优估计值
*/
int Kalman_Filter(kalman_t *pHandle, int x0, int y1) {
    /*
     * 当前的预测值
     * x的先验估计由上一个时间点的后验估计值和输入信息给出
     * X(k|k-1) = A X(k-1|k-1)
     */
    static int predictValue;
    predictValue = pHandle->A * y1;

    /*
     * 当前预测的协方差
     * 计算先验均方差
     * P(n|n-1) = A^2 * p(k-1|k-1) + Q
     */
    pHandle->P = pHandle->A * pHandle->A * pHandle->P + pHandle->Q;

    /*
     * 计算kalman增益
     * Kg(k)= P(k|k-1) H’ / (H P(k|k-1) H’ + R)
     */
    pHandle->Kg = 1000 * pHandle->P * pHandle->H / (pHandle->P * pHandle->H * pHandle->H + pHandle->R);
    /*
     * 当前最优值
     * 利用残余的信息改善对x(t)的估计，给出后验估计，这个值也就是输出
     * X(k|k)= X(k|k-1) + Kg(k) (Z(k)-H X(k|k-1))
     */

    y1 = (1000 * predictValue + (x0 - pHandle->H * predictValue) * pHandle->Kg) / 1000;
    /*
     * 更新当前最优的协方差
     * 计算后验均方差
     * P(k|k) = (1 - Kg(k)) * H) * P(k|k-1)
     */
    pHandle->P = (1000 - pHandle->Kg * pHandle->H) * pHandle->P / 1000;

    return y1;
}


/**
  * @brief  First-Order Low pass Filter.
  * @param  a   : Filter coefficient(0 ~ 1000).
  * @param  x0  : Current sampling value.
  * @param  y1  : Last filtered output value.
  * @retval     : Output value of this filter
  */

void LPF_Init(lpf_t *pHandle, uint16_t gain) {
    pHandle->gain = gain;
}

int LPF_Filter(lpf_t *pHandle, int x0, int y1) {
    return y1 + pHandle->gain * (x0 - y1) / 1000;
}


